#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/string.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/param_set.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "yolo_detect_message/msg/move_interface.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "yolo_detect_message/srv/service.hpp"
#include <geographic_msgs/msg/geo_pose_stamped.hpp>
#include <yolo_detect_message/srv/service.hpp>

#include <string>

using std::placeholders::_1;

// 定义一个名为 Communication 的类，它继承自 rclcpp::Node（ROS 2 的基础节点类）
class Communication : public rclcpp::Node {
public:
    // 构造函数，初始化通信节点，传入的参数包括 vehicle_type 和 vehicle_id
    Communication();
    double clampvalue(double &value, double min_val, double max_val);
    // 构造目标位置和运动的消息，用于控制无人机的具体动作
    mavros_msgs::msg::PositionTarget construct_target(
                double x, double y, double z, double vx, double vy, double vz,
                double afx, double afy, double afz, double yaw, double yaw_rate = 0);

    // 从四元数转换为偏航角
    double q2yaw(const geometry_msgs::msg::Quaternion &q);

    // 处理 ENU（东北天坐标系）下的位置信息命令
    void cmd_pose_enu_callback(const geometry_msgs::msg::Pose::SharedPtr msg);

    // 控制无人机进入悬停状态
    void hover();

    // 切换飞行模式
    bool flight_mode_switch();

    // 锁定无人机（解锁状态变为 false）
    bool disarm();

    // 解锁无人机（解锁状态变为 true）
    bool arm();

    // 处理 ENU 坐标系下的加速度命令
    void cmd_accel_enu_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    // 处理机体坐标系下的加速度命令
    void cmd_accel_flu_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    // 处理 ENU 坐标系下的速度命令
    void cmd_vel_enu_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    // 处理机体坐标系下的速度命令
    void cmd_vel_flu_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    // 处理收到的字符串命令
    void cmd_callback(const std_msgs::msg::String::SharedPtr msg);

    // 处理机体坐标系下的位置信息命令
    void cmd_pose_flu_callback(const geometry_msgs::msg::Pose::SharedPtr msg);

    void gps_callback(const yolo_detect_message::msg::Geolocation::SharedPtr msg);

    // 管理无人机的悬停状态转换
    void hover_state_transition(double x, double y, double z, double w);

    // 处理无人机的局部位置信息回调
    void local_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

    void state_callback(const mavros_msgs::msg::State::SharedPtr msg);
    // 发布目标运动信息到无人机控制器
    void publish_target();

    void info_topic();

   void initialize_parameters();

    // 初始化无人机的离线模式和解锁状态,设置无人机的offboard模式和解锁状态
    void init_offboard_and_arm();

    void set_offboard_mode();
    void arm_drone();

    void move_interface_callback(const yolo_detect_message::msg::MoveInterface::SharedPtr msg);

    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    void set_flight_mode(std::string cmd_string);
    bool is_arrival(const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2, double threshold );

    bool TargetYawCalibration(const geographic_msgs::msg::GeoPoseStamped target_motion, double &yaw);
    
      void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);
private:
    std::mutex last_request_mutex_;
    
    // 最近收到的命令
    std::mutex last_cmd_mutex;
    std::string last_cmd_;
    
    // 无人机的类型（例如 "drone" 或 "quadcopter"）
    std::string vehicle_type_;
    
    // 无人机的 ID
    std::string vehicle_id_;
    
    // 无人机的当前位置
    std::mutex position_mutex_;
    geometry_msgs::msg::Pose current_position_;
    
    // 无人机的当前偏航角
    double current_yaw_;
    
    // 悬停状态标志
    int hover_flag_;
    
    // 坐标系标志（例如 1 表示 ENU 坐标系）
    std::mutex coordinate_frame_mutex_;
    int coordinate_frame_;
    
    // 无人机的解锁状态（`true` 表示已解锁，`false` 表示未解锁）
    bool arm_state_;
    
    // 无人机的当前运动类型
    int motion_type_;
    
    // 当前的飞行模式（如 "AUTO" 或 "MANUAL"）
    // string MODE_PX4_MANUAL=MANUAL
    // string MODE_PX4_ACRO=ACRO
    // string MODE_PX4_ALTITUDE=ALTCTL
    // string MODE_PX4_POSITION=POSCTL
    // string MODE_PX4_OFFBOARD=OFFBOARD
    // string MODE_PX4_STABILIZED=STABILIZED
    // string MODE_PX4_RATTITUDE=RATTITUDE
    // string MODE_PX4_MISSION=AUTO.MISSION
    
    // string MODE_PX4_LOITER=AUTO.LOITER
    // string MODE_PX4_RTL=AUTO.RTL
    // string MODE_PX4_LAND=AUTO.LAND
    // string MODE_PX4_RTGS=AUTO.RTGS
    // string MODE_PX4_READY=AUTO.READY
    // string MODE_PX4_TAKEOFF=AUTO.TAKEOFF

    std::string flight_mode_;
    std::mutex control_type_mutex;

    bool control_type;

    std::mutex target_motion_mutex_;
    // 目标运动状态（用于控制无人机的目标位置和速度）
    mavros_msgs::msg::PositionTarget target_motion_;

    
    geometry_msgs::msg::PoseStamped init_target_motion_;
    
    std::mutex first_arrived_mutex;
    bool first_arrived;



    std::mutex target_gps_position_mutex_;
    // 目标位置的经纬度信息
    geographic_msgs::msg::GeoPoseStamped target_gps_position;

    // ROS 订阅者：用于接收无人机的局部位置信息
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr local_pose_sub_;
    
    // ROS 订阅者：用于接收控制命令
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr cmd_sub_;
    
    // ROS 订阅者：用于接收机体坐标系下的位置信息命令
    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cmd_pose_flu_sub_;

    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cmd_pose_enu_sub_;
    
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_flu_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_enu_sub_;

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_accel_flu_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_accel_enu_sub_;
    rclcpp::Subscription<yolo_detect_message::msg::Geolocation>::SharedPtr cmd_gps_move_sub_;

    

    rclcpp::Subscription<yolo_detect_message::msg::MoveInterface>::SharedPtr move_interface_;

  
  
    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_sub_;
    
    // ROS 发布者：用于发布目标运动状态
    rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr target_motion_pub_;

    // ROS 服务客户端：用于控制无人机的解锁状态
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arm_client_;
    
    // ROS 服务客户端：用于切换无人机的飞行模式
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr flight_mode_client_;

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    
    // 状态发布的定时器
    rclcpp::TimerBase::SharedPtr timer_;
    // 订阅当前的无人机的状态
    mavros_msgs::msg::State current_state_;
    // 进行状态的更行
    rclcpp::Time last_request_;
    // 定时器：用于定期发布目标运动状态
    rclcpp::TimerBase::SharedPtr publish_timer;

    // 第一次获取的无人机位置
    std::mutex init_received_position_mutxt_;
    bool init_received_position;

    geometry_msgs::msg::PoseStamped origin_position;

    std::string mission;
    bool init_flag_ = false;
    std::string namespaces;
    int vechile_nums;
    bool initial_setup_ = false;
    // 控制模式
    bool control_mode;
    double default_height_;
    std::atomic<bool> service_status{false};
    // 当前的位置
    geometry_msgs::msg::PoseStamped current_locate;

    double current_latitude_, current_longitude_, current_altitude_;
    bool hover_pos_setted_ = false;
    double hover_latitude_, hover_longitude_, hover_altitude_;

    // 确认是否离地面的高度
    std::mutex can_land_mutex;
    bool can_land;
    rclcpp::Publisher<geographic_msgs::msg::GeoPoseStamped>::SharedPtr gps_publisher_controler_;
    // 第一次接收到控制指令
    bool first_received_controller = false;
    
    std::string service_name;
    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;
};
